/*
 * QAgent.cpp
 *
 *  Created on: 6 mei 2013
 *      Author: Wietse
 */
#include "../Definitions.h"
#ifdef SOLO_AGENT


#include "QAgent.h"
#include "../Swarm.h"


/**
 * Default constructor
 */
QAgent::QAgent()
{
	decisionTimer 	= 0.0;
	targetID		= 0;
	target 			= Position(0.0,0.0);
}

/**
 * Preferred constructor
 */
QAgent::QAgent(Position* target)
{
	decisionTimer 	= 0.0;
	targetID		= rand() % NUM_OF_CRAWLERS;
	this->target 	= *target;
}

/**
 * Default destructor
 */
QAgent::~QAgent()
{
}

/**
 * Get the data from the data storage of the Quadcopter
 */
void QAgent::getData(Swarm* swarm, Position *currentPosition, std::vector<Data> *data)
{
	// Remove the old data about crawlers and quadcopters
	quadcopters.clear();
	crawlers.clear();

	// Loop through data vector
	for(unsigned int i = 0; i < data->size(); i++)
	{
		// Create a vehicleData struct
		VehicleData vehicleData;
		vehicleData.ID 			= data->at(i).getSenderID();
		vehicleData.position 	= *(data->at(i).getStart());
		vehicleData.range 		= data->at(i).getStart()->distance(currentPosition);
		vehicleData.angle 		= data->at(i).getStart()->angle(currentPosition);

		// Check of which type the vehicle is and store the data accordingly
		if(data->at(i).getSourceType() == QUADCOPTER)
		{
			quadcopters.push_back(vehicleData);
		}
		else if(data->at(i).getSourceType() == CRAWLER)
		{
			crawlers.push_back(vehicleData);
		}
	}
}

/**
 * Calculate the new target
 */
Position QAgent::calculateTarget(Swarm *swarm, Position *currentPosition, std::vector<Data> *data,  double deltaTime)
{
	// Increment decision timer
	decisionTimer += deltaTime;

	// Check if decision timer has expired and a new target has to be calculated
	if(decisionTimer > 1.0)
	{
		// Reset Decision Timer
		decisionTimer = 0.0;

		// Get the data that is available in this quadcopter
		getData(swarm, currentPosition, data);

		// Iterate over the crawler vector
		for(std::vector<int>::size_type i = 0; i < crawlers.size(); i++)
		{
			// Check if the crawler ID matches the target ID
			if(crawlers[i].ID == targetID)
			{
				// Calculate the absolute position of the crawler based on the relative position
				// to the quadcopter
				double x = crawlers.at(i).range * cos(crawlers.at(i).angle) + currentPosition->getX();
				double z = crawlers.at(i).range * sin(crawlers.at(i).angle) + currentPosition->getZ();

				// Set the target to the position of the crawler
				target = Position(x, z);

				return target;
			}
		}

		// Get random doubles for x and z
		double f = (double)rand() / RAND_MAX;
		double x = 0.0 + f * (100.0 - 0.0);

		f = (double)rand() / RAND_MAX;
		double z = 0.0 + f * (100.0 - 0.0);

		// Set a random target
		target = Position(x, z);
	}

	// Return the target
	return target;
}


#endif
